#include "modbus_slave.h"
#include <iostream>

modbus_mapping_t * ModbusCommunication::mb_mapping= NULL;   
ModbusCommunication::ModbusCommunication(const std::shared_ptr<rclcpp::Node> node):
    nh_(node), m_ctx(nullptr)
{
}

ModbusCommunication::~ModbusCommunication()
{
    //printf("ModbusMaster Destructor function entry: \n");
    m_quit_mutex.unlock();      
    m_thread.join();
    m_init_ok = false;
    modbus_close(m_ctx);
    modbus_free(m_ctx);
    modbus_mapping_free(mb_mapping);
    printf("ModbusCommunication Destructor function return: \n");
}

/*!
* \brief The uart or net parameter. 
* 获取链路层参数，Uart port baudrate,ip port等 .
*/
void ModbusCommunication::get_linkpara(std::string prefix)
{
    std::string temp;   
    std::string::size_type idx;
    idx= prefix.find("RS485");         
    if (idx == string::npos )       //不存在，即Xnet
    {
      m_use_backend = TCP;
      //assert(prefix == "/deviceTree/XNET1");
      std::string prefix_port= prefix + "/link_para";

      temp = prefix_port + "/ip";     
      nh_->get_parameter_or( temp, m_host_ip, std::string("192.168.1.12"));
      RCLCPP_INFO_STREAM(rclcpp::get_logger("Modbus"), "slave:" << temp << ":" << m_host_ip);

      temp = prefix_port + "/port";
      nh_->get_parameter_or(temp, m_host_port, 502);
      RCLCPP_INFO_STREAM(rclcpp::get_logger("Modbus"), "slave:" << temp << ":" << m_host_port);
    }
    else
    {
      m_use_backend = RTU;
      //assert(prefix == "/deviceTree/RS485_1");            
      // step2:获取串口通讯参数                             //prefix ="/deviceTree/XNET0  /protocol/master_para/slave1"
      std::string prefix_port= prefix + "/link_para";    //prefix ="/deviceTree/RS485_1/protocol/master_para/slave1"     

      // step2.1:获取串口设备描述符
      temp= prefix_port + "/name";
      nh_->get_parameter_or( temp, m_uart.port, std::string("/dev/ttyUSB0"));
      RCLCPP_INFO(rclcpp::get_logger("Modbus"), "%s: %s",temp.c_str(), m_uart.port.c_str());

      // step2.2:获取串口波特率
      temp= prefix_port + "/baudrate";
      nh_->get_parameter_or(temp, m_uart.baudrate, 9600);
      RCLCPP_INFO(rclcpp::get_logger("Modbus"), "%s: %d",temp.c_str(), m_uart.baudrate);

      // step2.3:获取停止位
      temp= prefix_port + "/data_bits";
      nh_->get_parameter_or(temp, m_uart.data_bits, 8);
      RCLCPP_INFO(rclcpp::get_logger("Modbus"), "%s: %d",temp.c_str(), m_uart.data_bits);

      // step2.4:获取校验位
      temp= prefix_port + "/patity";
      nh_->get_parameter_or<std::string>(temp, m_uart.patity, "N");
      RCLCPP_INFO(rclcpp::get_logger("Modbus"), "%s: %s",temp.c_str(), m_uart.patity.c_str());
      
      // step2.5:获取停止位
      temp= prefix_port + "/stop_bit";
      nh_->get_parameter_or(temp, m_uart.stop_bit, 1);
      RCLCPP_INFO(rclcpp::get_logger("Modbus"), "%s: %d",temp.c_str(), m_uart.stop_bit);
    }
    RCLCPP_INFO(rclcpp::get_logger("Modbus"), "init port or net OK");    

    prefix= prefix + "/protocol";
    // debug
    temp= prefix + "/reconnect";
    nh_->get_parameter_or(temp, m_reconnect, false);
    RCLCPP_INFO(rclcpp::get_logger("Modbus"), "%s:%d", temp.c_str(), m_reconnect);

    // debug
    temp= prefix + "/debug";
    nh_->get_parameter_or(temp, m_debug, false);
    RCLCPP_INFO(rclcpp::get_logger("Modbus"), "%s:%d", temp.c_str(), m_debug);

    // is_slave
    temp= prefix + "/is_slave";
    nh_->get_parameter_or(temp, m_is_slave, false);
    RCLCPP_INFO(rclcpp::get_logger("Modbus"), "%s:%d", temp.c_str(), m_is_slave);
}

int ModbusCommunication::modbusServer_set_Word16register( int reg_addr, const uint16_t value, uint16_t *pArr)
{
  if (mb_mapping == NULL && pArr == NULL)
    return -1;
  if (reg_addr >= MODBUS_MAX_READ_REGISTERS)
       return -2;
  pArr[reg_addr] = value;
  return 0;
}

int ModbusCommunication::set_bit_from_index( int reg_addr, int idx, uint16_t *pArr)
{
  if (mb_mapping == NULL && pArr == NULL)
    return -1;
  if (reg_addr >= MODBUS_MAX_READ_REGISTERS)
       return -2;

  int addr = (idx - 1) / 16;

  if ( addr < 15)
  {
    pArr[reg_addr + addr] |= (1 << ( (idx - 1) % 16));
  }
  else
  {
    RCLCPP_ERROR(rclcpp::get_logger("Modbus"), "addr outside 15 regs:%d", idx);
    return -3;
  }
    
  return 0;
}

int ModbusCommunication::modbusServer_set_Int32register( int reg_addr, const uint32_t value, uint16_t *pArr)
{
  if (mb_mapping == NULL && pArr == NULL)
    return -1;
  if (reg_addr >= MODBUS_MAX_READ_REGISTERS)
       return -2;
  MODBUS_SET_INT32_TO_INT16( pArr, reg_addr, value);
  return 0;
}
int ModbusCommunication::modbusServer_set_Long64register( int reg_addr, const uint64_t value, uint16_t *pArr)
{
  if (mb_mapping == NULL  && pArr == NULL)
    return -1;
  if (reg_addr >= MODBUS_MAX_READ_REGISTERS)
       return -2;
  MODBUS_SET_INT64_TO_INT16( pArr, reg_addr, value);
  return 0;
}
int ModbusCommunication::modbusServer_set_float( int reg_addr, const float f, uint16_t *pArr)
{
  if (mb_mapping == NULL && pArr == NULL)
    return -1;
  if (reg_addr >= MODBUS_MAX_READ_REGISTERS)
       return -2;
  
  modbus_set_float( f, pArr + reg_addr);
  return 0;
}
float ModbusCommunication::modbusServer_get_float( int reg_addr, uint16_t *pArr)
{
  if (mb_mapping == NULL && pArr == NULL)
    return -1;
  if (reg_addr >= MODBUS_MAX_READ_REGISTERS)
       return -2;
  
  return modbus_get_float( pArr + reg_addr);

}

uint16_t ModbusCommunication::modbusServer_get_register( int reg_addr, uint16_t *pArr)
{
  if (mb_mapping == NULL && pArr == NULL)
    return 0;
  if (reg_addr >= MODBUS_MAX_READ_REGISTERS)
       return 0;
  return pArr[reg_addr];
}

int ModbusCommunication::modbusServer_set_bit( int bit_addr, const uint8_t value, uint8_t *pArr)
{
  if (mb_mapping == NULL && pArr == NULL)
    return -1;
  if (bit_addr >= MODBUS_MAX_READ_BITS)
       return -2;

  modbus_set_bits_from_bytes(
  pArr, bit_addr, 1, &value);  
  return 0;
}
int ModbusCommunication::modbusServer_set_bits( int bit_addr, unsigned int nb_bits,const uint8_t *pValue, uint8_t *pArr)
{
    if (mb_mapping == NULL && pArr == NULL)
      return -1;
    if (bit_addr >= MODBUS_MAX_READ_BITS || (nb_bits + bit_addr) >= MODBUS_MAX_READ_BITS)
         return -2;
    
    modbus_set_bits_from_bytes(
    pArr, bit_addr, nb_bits, pValue);
    return 0;

}

bool ModbusCommunication::modbusServer_get_bit( int reg_addr, uint8_t *pArr)
{
  if (mb_mapping == NULL && pArr == NULL)
    return -1;
  if (reg_addr >= MODBUS_MAX_READ_BITS)
       return -2;
  
  return modbus_get_byte_from_bits( pArr, reg_addr, 1);
}

bool ModbusCommunication::write_output_io_bits(const int& start_addr, const int& count_io_bits, const uint8_t *need_set_levels)
{
    std::lock_guard<std::mutex> locker(m_mutex);
    int ret = -1;
    if (m_ctx == nullptr)
    {
        RCLCPP_INFO(rclcpp::get_logger("Modbus"), "write_bits cts is nullptr");
        return false;
    }
    ret = modbus_write_bits(m_ctx, start_addr, count_io_bits, need_set_levels); // 0x0f
    if(ret == -1){
        return false;
    }
    return true;
}

bool ModbusCommunication::write_single_io_bit(const int &addr, const bool& level)
{
    std::lock_guard<std::mutex> locker(m_mutex);
    int ret = -1;
    if (m_ctx == nullptr)
    {
        RCLCPP_INFO(rclcpp::get_logger("Modbus"), "write_single_io_bit cts is nullptr");
        return false;
    }
    ret = modbus_write_bit(m_ctx, addr, level);
    if(ret == -1){
        return false;
    }
    return true;
}


bool ModbusCommunication::read_input_io_bits(const int &start_addr, const int &count_io_bits, uint8_t *ret_dest_status)
{
    std::lock_guard<std::mutex> locker(m_mutex);
    if (m_ctx == nullptr)
    {
        RCLCPP_INFO(rclcpp::get_logger("Modbus"), "read_input_io_bits cts is nullptr");
        return false;
    }
    // const int count_io = 0x10;
    // uint8_t dest[count_io];
    int ret = modbus_read_input_bits(m_ctx, start_addr, count_io_bits, ret_dest_status);
    modbus_flush(m_ctx);
    if (ret == -1)
    {
        fprintf(stderr, "read input bits failed : %s\n", modbus_strerror(errno));
        return false;
    }
    return true;
}

